/*
 * rosa_queues.c
 *
 * Created: 2014-12-03 13:47:12
 *  Author: eds04001
 */
#include <stdlib.h>

#include "kernel/rosa_task.h"
#include "kernel/rosa_scheduler.h"
#include "kernel/rosa_queues.h"
#include "kernel/rosa_util.h"

tcb *readyQueueHead = NULL;
tcb *waitingQueueHead = NULL;
tcb *deleteQueue = NULL;
tcb *blockedQueueHead = NULL;

/// Removes specified task from specified queue
void rosa_removeFromQueue(tcb *task, tcb **queueHead)
{
	if(task == *queueHead)
	{
		blockedQueueHead = task->nexttcb;
		task->nexttcb = NULL;
	}
	else
	{
		tcb *iterator = *queueHead;
		while(iterator->nexttcb)
		{
			if(iterator->nexttcb == task)
			{
				iterator->nexttcb = task->nexttcb;
				task->nexttcb = NULL;
				return;
			}
		}
		breakpoint;		// Error: Could not find task in queue
	}
}

///////////////////////////
// Ready queue - sorted on dynamicPriority, highest first
///////////////////////////

/// Returns true if the ready queue is in the right order
static bool readyQueueIsInOrder(void)
{
	int previousTasksPriority = readyQueueHead->dynamicPriority;
	for(tcb *iterator = readyQueueHead; iterator != NULL; iterator = iterator->nexttcb)
	{
		if(previousTasksPriority < iterator->dynamicPriority)
			return false;
		previousTasksPriority = iterator->dynamicPriority;
	}
	return true;	
}

/// Adds the specified task to the ready queue. The task is placed
/// just before the first task with a lower dynamic priority.
void rosa_addToReadyQueue(tcb *newTask)
{
	if (readyQueueHead == NULL || newTask->dynamicPriority > readyQueueHead->dynamicPriority)
	{
		newTask->nexttcb = readyQueueHead;
		readyQueueHead = newTask;
	}
	else
	{
		tcb *temp = readyQueueHead;
		while (temp->nexttcb != NULL)
		{
			// Find the first task with a lower priority
			if (newTask->dynamicPriority > temp->nexttcb->dynamicPriority)
			{
				newTask->nexttcb = temp->nexttcb;
				temp->nexttcb = newTask;
				goto done;
			}
			temp = temp->nexttcb;
		}
		temp->nexttcb = newTask;
		newTask->nexttcb = NULL;
	}
	done:
	assert(readyQueueIsInOrder());
}

/// Adds the specified task to the ready queue. The task is placed first
/// in the ready queue
void rosa_addToReadyQueueFront(tcb *newTask)
{
	assert(readyQueueHead);
	assert(readyQueueHead->nexttcb);
	newTask->nexttcb = readyQueueHead->nexttcb;
	readyQueueHead->nexttcb = newTask;
	assert(readyQueueIsInOrder());
}

/// Removes the first task in the ready queue (the one with the highest priority) and returns it
tcb* rosa_popHighestPriorityTaskFromReadyQueue()
{
	tcb *temp = readyQueueHead;
	
	readyQueueHead = readyQueueHead->nexttcb;	
	temp->nexttcb = NULL;
	return temp;
}

///////////////////////////
// Waiting queue, sorted on wakeUpTime, earliest wake up first
///////////////////////////

static bool queueIsSortedOnWakeupTime(tcb *queueHead)
{
	int previousTasksWakeupTime = queueHead->wakeUpTime;
	for(tcb *iterator = queueHead; iterator != NULL; iterator = iterator->nexttcb)
	{
		if(previousTasksWakeupTime > iterator->wakeUpTime)
			return false;
		previousTasksWakeupTime = iterator->wakeUpTime;
	}
	return true;	
}

/// Adds specified task to the waiting queue, with the specified wakeup time
void rosa_addToWaitingQueue(tcb *newTask)
{
	if (waitingQueueHead == NULL || newTask->wakeUpTime < waitingQueueHead->wakeUpTime)
	{
		newTask->nexttcb = waitingQueueHead;
		waitingQueueHead = newTask;
	}
	else
	{
		tcb *temp = waitingQueueHead;
		while (temp->nexttcb != NULL)
		{
			if (newTask->wakeUpTime < temp->nexttcb->wakeUpTime)
			{
				newTask->nexttcb = temp->nexttcb;
				temp->nexttcb = newTask;
				goto done;
			}
			temp = temp->nexttcb;
		}
		temp->nexttcb = newTask;
		newTask->nexttcb = NULL;
	}
	done:
	assert(queueIsSortedOnWakeupTime(waitingQueueHead));
}

int rosa_moveFirstTaskFromWaitingToReady()
{
	tcb *temp = waitingQueueHead;
	waitingQueueHead = waitingQueueHead->nexttcb;
	temp->nexttcb = NULL;

	rosa_addToReadyQueue(temp);
	return 0;
}

///////////////////////////
// Delete queue, not sorted
///////////////////////////

int rosa_addToDelete(tcb  *task)
{
	task->nexttcb = deleteQueue;
	deleteQueue = task;
	return 0;
}

tcb* rosa_removeFromDelete()
{
	if(deleteQueue != NULL)
	{		
		tcb *temp = deleteQueue;
		temp->nexttcb = NULL;
		deleteQueue = deleteQueue->nexttcb;
		return temp;
	}
	else
		return NULL;
}

///////////////////////////
// Blocked queue, sorted on wakeUpTime, earliest wakeup first.
// tasks that block infinitely are stored at the end
///////////////////////////

/// Adds the specified task to the blocked queue
void rosa_addToBlocked(tcb  *newTask)
{
	newTask->nexttcb = blockedQueueHead;
	blockedQueueHead = newTask;
}

static unsigned int queueLength(tcb *head)
{
	unsigned int result = 0;
	for(tcb *iterator = head; iterator != NULL; iterator = iterator->nexttcb)
	{
		result++;
	}
	return result;
}

unsigned int readyQueueLength()
{
	return queueLength(readyQueueHead);
}

unsigned int waitingQueueLength()
{
	return queueLength(waitingQueueHead);
}

unsigned int deleteQueueLength()
{
	return queueLength(deleteQueue);
}

/// Returns the length of the blocked queue
int rosa_blockedQueueLength()
{
	return queueLength(blockedQueueHead);
}